#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import numpy as np
import matplotlib.pyplot as plt
import matplotlib.animation as animation
from scipy.interpolate import CubicSpline

# 创建参考线上的基础点
wx = [0.0, 10.0, 20.0, 30.0, 40.0, 50.0]
wy = [0.0, -3.0, 4.0, 2.5, 0.0, -1.0]

# 构建 s（累计距离）坐标
s = [0]
for i in range(1, len(wx)):
    dx = wx[i] - wx[i - 1]
    dy = wy[i] - wy[i - 1]
    s.append(s[-1] + np.hypot(dx, dy))
s = np.array(s)

# 使用 Cubic Spline 构造参考线（s -> x, s -> y）
sx = CubicSpline(s, wx)
sy = CubicSpline(s, wy)

# Frenet 坐标路径：s 沿参考线前进，d 为横向偏移
def frenet_to_global(s_vals, d_vals):
    x_vals, y_vals = [], []
    for si, di in zip(s_vals, d_vals):
        dx = sx(si)
        dy = sy(si)
        # 计算参考线切线方向
        dx_ds = sx.derivative()(si)
        dy_ds = sy.derivative()(si)
        heading = np.arctan2(dy_ds, dx_ds)
        # Frenet d 偏移
        x = dx - di * np.sin(heading)
        y = dy + di * np.cos(heading)
        x_vals.append(x)
        y_vals.append(y)
    return x_vals, y_vals

# 构造一条 frenet 路径：从 s=0 开始，每隔1米取一个点，共取50个点
s_vals = np.linspace(0, s[-1], num=100)
d_vals = 2.0 * np.sin(s_vals / 5.0)  # 模拟车辆左右偏移的路径

# 转换为全局坐标系
x_vals, y_vals = frenet_to_global(s_vals, d_vals)

# 可视化 + 动画
fig, ax = plt.subplots()
ax.plot(wx, wy, 'ko-', label='Waypoints')
ax.plot(sx(s), sy(s), 'g--', label='Reference line')
line, = ax.plot([], [], 'r-', lw=2, label='Planned path')
car_dot, = ax.plot([], [], 'bo', markersize=6)

ax.set_xlim(-10, 60)
ax.set_ylim(-20, 20)
ax.set_aspect('equal')
ax.grid(True)
ax.legend()

def init():
    line.set_data([], [])
    car_dot.set_data([], [])
    return line, car_dot

def animate(i):
    line.set_data(x_vals[:i], y_vals[:i])
    if i < len(x_vals):
        car_dot.set_data(x_vals[i], y_vals[i])
    return line, car_dot

ani = animation.FuncAnimation(fig, animate, frames=len(x_vals), init_func=init,
                              interval=50, blit=True, repeat=False)

plt.title("Frenet Path Planning Visualization")
plt.show()
